#pragma once
#include "FunctionalEntity.h"
#include "IEntityMovement.h"
#include "Terrain.h"
#include <queue>

namespace game
{
		template <typename t_state>
	class DynamicFunctionalEntity:public FunctionalEntity<t_state>, public IEntityMovement
	{
		//IEntityMovement fields//
		float						_speed;
		std::queue<sf::Vector2u>	_future_move_pos;
		game::TerrainManager*		_terrain_manager;
		game::IEntityMoveState		_move_state;
	public:
		DynamicFunctionalEntity(t_state state,sf::Vector2f pos, animSprite * sprite, 
								sf::RenderWindow * window,std::string name, int object_type_id,
								game::TerrainManager * tm)

								:FunctionalEntity(state,pos,sprite,window,name,object_type_id)
		{
			_terrain_manager = tm;
			_speed = 3.513f;
		}
		
		//IEntityMovement members//
		void					set_terrain_manager(game::TerrainManager * tm){_terrain_manager =tm;}
		game::TerrainManager *	get_terrain_manager(){return _terrain_manager;}
		void	set_speed(float speed){_speed = speed;}
		float	get_speed(){return _speed;}
		virtual void				set_move_state(IEntityMoveState state){_move_state = state;}
		virtual IEntityMoveState	get_move_state(){return _move_state;}
		sf::Vector2u get_current_grid_position()
		{
			if (_future_move_pos.empty())
			{
				return sf::Vector2u(get_position().x,get_position().y);
			}
			else return _future_move_pos.front();
		}
		sf::Vector2u get_last_future_pos()
		{
						if (!_future_move_pos.empty())
			{
				return _future_move_pos.back();
			}
			else {return sf::Vector2u(get_position().x,get_position().y);}
		}
		void MoveToPoint(sf::Vector2f goalf)
		{
			//snap to grid
			sf::Vector2u goal(goalf.x,goalf.y);
			MoveToPoint(goal);
		}
		void MoveToPoint(sf::Vector2u goal)
		{
			//snap to grid
			goal.x -= (goal.x%GRIDSIZE);	
			goal.y -= (goal.y%GRIDSIZE);
			//convert x-cord to tile id
			goal.x /= game::GRIDSIZE;	
			goal.y /= game::GRIDSIZE;
			sf::Vector2u final_point = get_last_future_pos();
			//convert x-cord to tile id
			final_point.x /= game::GRIDSIZE;	
			final_point.y /= game::GRIDSIZE;
			int map[32][32];
			//fill map with air//
			for(int j = 0; j < 32; j++)for (int i = 0; i < 32; i++)
			{
				if(get_terrain_manager()->is_grid_pathable(i,j))
				{
					map[i][j] = 0;
				}
				else
				{
					map[i][j] = 1;
				}
			}
			//for (int i = 0; i < 10; i++)
			//{
			//	map[i][3] = 1;
			//}
			//get direction string//

			bool shift_key = false;

			if(!shift_key && !_future_move_pos.empty())
			{
				sf::Vector2u fpos = sf::Vector2u(_future_move_pos.front().x,_future_move_pos.front().y);// _future_move_pos.front().x;
				while(_future_move_pos.empty() == false)
				{_future_move_pos.pop();}
				_future_move_pos.push(fpos);

				final_point.x = fpos.x / game::GRIDSIZE;	
				final_point.y = fpos.y/ game::GRIDSIZE;

			}
			std::string path = game::PathFinder::GetPath(final_point.x,final_point.y,goal.x,goal.y,map);
			const int size = path.size();
			for (int i = 0; i < size; i++)
			{
				string o = "";
				if		(path[i] == '0'){o = "Right";}
				else if (path[i] == '1'){ o = "Down";}
				else if (path[i] == '2'){ o = "Left";}
				else if (path[i] == '3'){ o = "Up";}
				if (o!=""){MoveToNextCell(o);}
			}
		}
		void MoveToNextCell(std::string key)
		{
			sf::Vector2u cur_pos = get_last_future_pos();
			IEntityMoveState state = get_move_state();
			if		(key == "Up" )	{_future_move_pos.push(sf::Vector2u(cur_pos.x, cur_pos.y -= game::GRIDSIZE));state = S_I_ENTITY_MOVE_STATE_MOVING;}
			else if (key == "Down")	{_future_move_pos.push(sf::Vector2u(cur_pos.x ,cur_pos.y += game::GRIDSIZE));state = S_I_ENTITY_MOVE_STATE_MOVING;}
			else if (key == "Right"){_future_move_pos.push(sf::Vector2u(cur_pos.x += game::GRIDSIZE, cur_pos.y));state = S_I_ENTITY_MOVE_STATE_MOVING;}
			else if (key == "Left")	{_future_move_pos.push(sf::Vector2u(cur_pos.x -= game::GRIDSIZE, cur_pos.y));state = S_I_ENTITY_MOVE_STATE_MOVING;}
		}
		virtual void Init()
		{
			int x = (int)get_position().x - ((int)get_position().x % GRIDSIZE);	
			int y = (int)get_position().y - ((int)get_position().y % GRIDSIZE);
			set_position(sf::Vector2f(x,y));
			std::cout << "Init" << std::endl;
		}
		void Update()
		{
			if (!_future_move_pos.empty())
			{
				//move towards pos
				sf::Vector2u temp_pos = _future_move_pos.front();
				float dx = (float)temp_pos.x - get_position().x;
				float dy = (float)temp_pos.y - get_position().y;
				float angle = std::atan2(dy,dx);
				set_position(sf::Vector2f(get_position().x + get_speed()* std::cos(angle),get_position().y));
				set_position(sf::Vector2f(get_position().x,get_position().y + get_speed()* std::sin(angle)));

				float distance_squared = dx*dx + dy * dy;	//distance to target point
				if (get_position() == sf::Vector2f(temp_pos.x,temp_pos.y))
				{
					_future_move_pos.pop();
					if(_future_move_pos.empty())
					set_move_state(game::S_I_ENTITY_MOVE_STATE_IDLE);
				}
				if (distance_squared < get_speed() * get_speed())
				{
					set_position(sf::Vector2f(temp_pos.x,temp_pos.y));
					_future_move_pos.pop();
					if(_future_move_pos.empty())
					set_move_state(game::S_I_ENTITY_MOVE_STATE_IDLE);
				}
			}

		}
	};


}